import rosbag
from colorama import Fore, Style
import os
import sys
import warnings
import def_function

# 获取当前脚本的完整路径
script_path = os.path.abspath(sys.argv[0])
# 从完整路径中获取目录
script_dir = os.path.dirname(script_path)
# 从完整路径中分离出文件名
script_name = os.path.basename(script_path)
# 使用 splitext() 函数分离文件名和扩展名
script_name_without_extension, extension = os.path.splitext(script_name)

print(Fore.YELLOW+f"python script name:", sys.argv[0])
if os.getenv('DEBUG') == 'true':
    python_param = '/mnt/01_code/python/rosbag_analysis/dataspace/ROCKY/outputPL077159_recording_dfdi_console_manual_recording_20230825-135935_20230825-140216_0_augmented_filter.bag'
else:
    if len(sys.argv)!=2:
        print(Fore.RED+f"you need python params!")
        sys.exit()
    python_param = script_dir + "/" + sys.argv[1]
    if os.path.isabs(sys.argv[1]):
        python_param = sys.argv[1]
rosbag_data = rosbag.Bag(python_param)

# 获取类型和主题信息
# rosbag_info = rosbag_data.get_type_and_topic_info()
# info.topics 是一个字典，它的键是 topic 的名字
# all_topics = list(rosbag_info.topics.keys())

topics = ['/vehicle/chassis_report', 
          '/vehicle/wheel_report', 
          '/vehicle/body_report', 
          '/node_status/vehicle_service', 
          '/msd/function_module_status',
          '/mla/egopose', 
          '/mb/sensor/imu', 
          '/msd/function_module_status', 
          '/msd/endpoint/control_command']
error_msg_arry = []
dict_msg = dict()
arry_error_info = list()
dict_error_info = dict()

class diff_info:
    def __init__(self):
        self.seq = []
        self.header = []
dict_dff_info = dict()
dict_judge = dict()

cnt = 0
for msg_topic, msg, t in rosbag_data.read_messages(topics=topics):
    if dict_msg.get(msg_topic) is not None:
        #计算差值
        if dict_dff_info.get(msg_topic) is None:
            dict_dff_info.setdefault(msg_topic, diff_info())
        dff = dict_dff_info[msg_topic]
        dff.header.append((msg.header.stamp.secs * 1000000 + msg.header.stamp.nsecs / 1000) - (dict_msg[msg_topic].header.stamp.secs * 1000000 + dict_msg[msg_topic].header.stamp.nsecs / 1000))

    for key, value in dict_dff_info.items():
        error_type = "数据偶发性断流"
        if dict_error_info.get(error_type+key) is None:
            dict_error_info.setdefault(error_type+key, def_function.class_data.error_info())
        err_info = dict_error_info[error_type+key]
        judge_condition = "前后帧header时间戳差值大于200ms"
        result = value.header[len(value.header)-1] > 200*1000
        message = "[" + error_type + "{" + judge_condition + "}]" +  key + " 上一帧:" + str(dict_msg[key].header.stamp.secs * 1000000 + dict_msg[key].header.stamp.nsecs / 1000) + " 当前帧:" + str(msg.header.stamp.secs * 1000000 + msg.header.stamp.nsecs / 1000) + " 差值:" + str(value.header[len(value.header)-1])
        def_function.class_data.update_error_info(t.to_sec(), result, message, dict_error_info, error_type+key, arry_error_info)
        
    dict_msg[msg_topic] = msg
    ################################################################################################
        
for key, value in dict_msg.items():
    error_type = "数据断流到包尾(数据偶发性断流无发检测持续到包结束都没有数据问题)"
    if dict_error_info.get(error_type+key) is None:
        dict_error_info.setdefault(error_type+key, def_function.class_data.error_info())
    err_info = dict_error_info[error_type+key]
    judge_condition = "包尾帧数据header时间戳与topic数据最后一次header时间戳差值大于200ms"
    time_diff = (msg.header.stamp.secs * 1000000 + msg.header.stamp.nsecs / 1000) - (value.header.stamp.secs * 1000000 + value.header.stamp.nsecs / 1000)
    result = time_diff > 200*1000
    message = "[" + error_type + "{" + judge_condition + "}]" +  key + "包尾帧数据 :" + str(msg.header.stamp.secs * 1000000 + msg.header.stamp.nsecs / 1000) + " topic数据最后一次:" + str(value.header.stamp.secs * 1000000 + value.header.stamp.nsecs / 1000) + " 差值:" + str(time_diff)
    def_function.class_data.update_error_info(t.to_sec(), result, message, dict_error_info, error_type+key, arry_error_info)
################################################################################################
rosbag_data.close()
list_print = list()
for value in dict_error_info.values():
    if (0 != value.start_time and 0 == value.end_time):
        list_print.append("[" + str(value.start_time) + "-" + "bag_end]" + value.message)
for item in arry_error_info:
    list_print.append("[" + str(item.start_time) + "-" + str(item.end_time) + "@diff:" + "{:.3f}".format(item.end_time - item.start_time) + "]" + item.message)
if list_print:
    print(Fore.RED+f"", script_name_without_extension)
for item in list_print:
    print(Fore.CYAN+f"", item)

